#include "Config.h"
#include "CANBus.h"
#include "MotorController.h"
#include "Keyboard.h"
#include "WebServer.h"
#include "Camera.h"  // 新增：USB摄像头头文件
#include "WebDirectionLogic.h"  // 新增：方向逻辑头文件
#include <thread>
#include <atomic>
#include <iostream>
#include <chrono>
#include <signal.h>  // 需包含信号头文件


using namespace httplib;
using namespace jetcar;

jetcar::MotorController* g_motor = nullptr;
UsbCamera* g_camera = nullptr;
httplib::Server* g_svr = nullptr;
std::thread* g_web_thread = nullptr;
std::atomic<bool> g_running(true);  // 程序运行标志

// 信号处理函数（捕获 Ctrl+C 退出信号）
void signalHandler(int signum) {
    std::cout << "\n[System] 收到终止信号（信号码：" << signum << "），开始释放资源..." << std::endl;
    g_running = false;  // 停止所有循环

    // 1. 停止 Web 服务
    if (g_svr != nullptr) {
        g_svr->stop();
    }
    if (g_web_thread != nullptr && g_web_thread->joinable()) {
        g_web_thread->join();
        std::cout << "[Web] 服务已停止" << std::endl;
    }

    // 2. 停止电机
    if (g_motor != nullptr) {
        g_motor->stopAll();
        std::cout << "[Motor] 电机已停止" << std::endl;
    }

    // 3. 停止摄像头
    if (g_camera != nullptr) {
        try {
            g_camera->stop();
            std::cout << "[Camera] 摄像头已停止" << std::endl;
        } catch (...) {
            std::cerr << "[Camera] 摄像头停止失败" << std::endl;
        }
    }

    // 4. 关闭 CAN 接口
    jetcar::closeCAN();
    std::cout << "[CAN] CAN 接口已关闭" << std::endl;

    std::cout << "[System] 所有资源释放完成，程序退出" << std::endl;
    exit(signum);  // 正常退出程序
}

int main(int argc, char** argv) {
    // 注册信号处理：处理 Ctrl+C（SIGINT）、kill（SIGTERM）、强制关闭（SIGABRT）
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    signal(SIGABRT, signalHandler);

    Config cfg;
    if (!loadConfig("./config/config.json", cfg)) {
        std::cerr << "Cannot load config.json\n";
        return -1;
    }

    // 新增：初始化 USB 摄像头（配置从 config.json 读取）
    UsbCamera camera(
        cfg.camera_device,    // 摄像头设备节点（如 /dev/video0）
        cfg.camera_width,     // 分辨率宽（如 640）
        cfg.camera_height,    // 分辨率高（如 480）
        cfg.camera_fps        // 帧率（如 30）
    );
    if (!camera.start()) {
        std::cerr << "[Warning] USB Camera init failed! Web will not show video feed.\n";
        // 摄像头启动失败不影响其他功能，继续运行
    }
    g_camera = &camera;

    // 初始化 CAN 总线（保留原有逻辑）
    if (!jetcar::initCAN(cfg.can_device)) {
        std::cerr << "CAN init failed\n";
        return -1;
    }

    // 初始化电机控制器（保留原有逻辑）
    jetcar::MotorController motor(cfg);
    motor.start();
    g_motor = &motor;
#if 0
    // 保留原有电机测试逻辑（方向枚举需与新的6方向兼容，后面会修改）
    std::cout << "\n=== Motor Test ===\n";
    std::cout << "1. Forward 2s → Stop → Backward 2s → Stop\n";
    // 前进（对应新方向码 1）
    motor.setTarget(jetcar::MotorController::Direction::FORWARD, cfg.default_speed);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    motor.stop();
    std::this_thread::sleep_for(std::chrono::seconds(3));
    // 后退（对应新方向码 2）
    motor.setTarget(jetcar::MotorController::Direction::BACKWARD, cfg.default_speed);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    motor.stop();
    std::this_thread::sleep_for(std::chrono::seconds(3));

    // 左前（新增，对应新方向码 3）
    motor.setTarget(jetcar::MotorController::Direction::LEFT_FRONT, cfg.default_speed);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    motor.stop();
    std::this_thread::sleep_for(std::chrono::seconds(3));
    
    // 右前（新增，对应新方向码 4）
    motor.setTarget(jetcar::MotorController::Direction::RIGHT_FRONT, cfg.default_speed);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    motor.stop();
    std::this_thread::sleep_for(std::chrono::seconds(3));

    // 左后（新增，对应新方向码 5）
    motor.setTarget(jetcar::MotorController::Direction::LEFT_BACK, cfg.default_speed);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    motor.stop();
    std::this_thread::sleep_for(std::chrono::seconds(3));
    
    // 右后（新增，对应新方向码 6）
    motor.setTarget(jetcar::MotorController::Direction::RIGHT_BACK, cfg.default_speed);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    motor.stop();
    std::this_thread::sleep_for(std::chrono::seconds(3));

    std::cout << "Test complete.\n";
    motor.stopAll();
    jetcar::closeCAN();
    return 0;

#else
    // 核心修改：对齐方向类型和初始化摄像头
    std::atomic<int> speed(cfg.default_speed);
    // 原有 direction 是 int 类型（0=停止，1=前进...），与新的 DirectionCode 枚举值兼容
    std::atomic<int> direction(0);

    
    // 启动 Web 服务（修改参数：新增 camera 引用，direction 保持 int 类型兼容）
    httplib::Server svr;
    g_svr = &svr;
    std::thread webThread(
        jetcar::startWebServer,  // WebServer 函数（需在 WebServer.h 中调整参数类型）
        std::ref(svr), 
        std::ref(speed),
        std::ref(direction),
        std::ref(cfg),
        std::ref(motor),
        std::ref(camera)
    );
    g_web_thread = &webThread;

    // 保留原有键盘控制逻辑（需确保 keyboardLoop 中的方向值与新6方向码兼容）
    keyboardLoop(speed, direction, cfg, motor);

     // 5. 资源释放（仅当 keyboardLoop 退出时执行）
    std::cout << "[System] 开始释放资源..." << std::endl;
    motor.stopAll();  // 停止电机
    camera.stop();    // 停止摄像头
    svr.stop();       // 停止 Web 服务
    std::cout << "[svr] stop()\n";
    if (webThread.joinable()) {
        webThread.join();  // 等待 Web 线程退出（避免资源泄漏）
        std::cout << "Web 线程退出\n";
    }
    jetcar::closeCAN();  // 关闭 CAN 总线
    std::cout << "CAN 关闭\n";
    std::this_thread::sleep_for(std::chrono::seconds(1));
    std::cout << "Exit successfully\n";

#endif
    return 0;
}